#ifndef BC_BODYPOS_TASK_H
#define BC_BODYPOS_TASK_H

#include "srpq_controller/AdvanceControllers/BalanceController/BalanceCtrlTask.hpp"

template <typename T>
class BodyPosTask : public BalanceCtrlTask<T> {
public:
    BodyPosTask(){}
    ~BodyPosTask(){}
    virtual void  task(ControlFSMData<T>& data, BalanceCtrlData<T>* ctrl_data, BCoutputData<T>* BCout, int leg); 
private:
    bool updateFlag = false;
    Vec3<T> pos_world_des;
    T fixR = 0.05; // the length to correct position
};

#endif
